clc; close all; clear all;

dt = 0.01;

%% Unicycle Model
sys = ICtSystem(...
    'StateEquation', @(t,x,u,varargin) [
    u(1)*cos(x(3));
    u(1)*sin(x(3));
    u(2)],...
    'nx',3,'nu',2 ...
);

sys.initialCondition = {[15;15;-pi/2],-[15;15;-pi/2],[15;-15;pi],[-15;15;-pi/2]};

sys.controller = TrackingController_ECC13(...
    @(t) 10*[sin(0.1*t); cos(0.1*t)] , ... % c
    @(t)    [cos(0.1*t);-sin(0.1*t)] , ... % cDot
    eye(2)                           , ... % K
    [1;0] );

va = VirtualArena(sys,...
    'StoppingCriteria'  , @(t,sysList)t>70,...
    'DiscretizationStep', dt,...
    'PlottingStep'      , 1, ...
    'StepPlotFunction'  , @ex01StepPlotFunction ...
    );

log = va.run();